#include "servo.h"
#include "delay.h"
#include "stm32f10x.h"
#include "stm32f10x_conf.h"

#define IS_VALID_SERVO(servo)       (servo >= 1 && servo <= 2)
#define IS_VALID_ANGLE(angle)       (angle >= 0 && angle <= MAX_ANGLE)
#define IS_VALID_ANGLE_SPEED(speed) (speed >= 0 && speed <= MAX_ANGLE_SPEED)

/* PWM clock and period */
#define PWM_50HZ_CLOCK    (SystemCoreClock / 10)
#define PWM_50HZ_PERIOD   (PWM_50HZ_CLOCK/50)
/* PWM duty for angle 0 */
#define ANGLE_0_PWM_DUTY  (PWM_50HZ_PERIOD * ANGLE_0_DUTY_US / PERIOD_50HZ)

/* Convert angle to PWM duty and vise versa */
#define ANGLE_TO_US(angle)  ((ANGLE_180_DUTY_US - ANGLE_0_DUTY_US) * (angle) / MAX_ANGLE + ANGLE_0_DUTY_US)
#define ANGLE_TO_PWM_DUTY(angle) (PWM_50HZ_PERIOD * ANGLE_TO_US(angle) / PERIOD_50HZ)  
#define US_TO_ANGLE(us)     (MAX_ANGLE * (us - ANGLE_0_DUTY_US) / (ANGLE_180_DUTY_US - ANGLE_0_DUTY_US))
#define PWM_DUTY_TO_ANGLE(duty) US_TO_ANGLE(PERIOD_50HZ * duty / PWM_50HZ_PERIOD)

void PWM_Configuration_50Hz(TIM_TypeDef* TIMx)
{
  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
  TIM_OCInitTypeDef  TIM_OCInitStructure;   
  
  /* Compute the prescaler value */
  uint16_t PrescalerValue = (uint16_t)(SystemCoreClock/PWM_50HZ_CLOCK)  - 1;
  /* Time base configuration */
  TIM_TimeBaseStructure.TIM_Period = PWM_50HZ_PERIOD - 1;
  TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue;
  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;

  TIM_TimeBaseInit(TIMx, &TIM_TimeBaseStructure);

  /* PWM1 Mode configuration: Channel1 to Channel4 */
  TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
  
  TIM_OCInitStructure.TIM_Pulse = ANGLE_0_PWM_DUTY;
  TIM_OC3Init(TIMx, &TIM_OCInitStructure);
  TIM_OC3PreloadConfig(TIMx, TIM_OCPreload_Enable);
  TIM_OC4Init(TIMx, &TIM_OCInitStructure);
  TIM_OC4PreloadConfig(TIMx, TIM_OCPreload_Enable);

  TIM_ARRPreloadConfig(TIMx, ENABLE);

  /* TIMx enable counter */
  TIM_Cmd(TIMx, ENABLE);
}

/* Set servo position at max speed */
int SetServoMax(int channel, int angle)
{
  assert_param(IS_VALID_SERVO(channel));
  assert_param(IS_VALID_ANGLE(angle));
  int pwm_duty = ANGLE_TO_PWM_DUTY(angle);
  
  switch(channel)
  {
  case 1:
    TIM2->CCR3 = pwm_duty;
    break;
  case 2:
    TIM2->CCR4 = pwm_duty;
    break;
  }
  
  return GetServo(channel);
}

/* Target angle for scheduled set of servo`s position */
int servo1_target_angle, servo2_target_angle;

/* Do angle step for the specified servo */
int DoAngleStep(int channel, int *target)
{
  if(*target < 0)
    return 0;
  
  /* Calculate delta and new angle */
  int now_angle = GetServo(channel);
  int angle_delta = *target - now_angle;
  if(angle_delta < 0)
  {
    now_angle -= ANGLE_STEP;
    angle_delta = -angle_delta;
  }
  else
    now_angle += ANGLE_STEP;
  
  /* Set new angle */
  SetServoMax(channel, now_angle);
    
  /* Reschedule if now angle is not near target angle */
  if(angle_delta > ANGLE_DELTA)
    return 1;
  else
  {
    *target = -1;
    return 0;
  }
}

/* Angle step scheduler handler for servo 1 */
int DoAngleStep1(void)
{
  return DoAngleStep(1, &servo1_target_angle);
}

/* Angle step scheduler handler for servo 2 */
int DoAngleStep2(void)
{
  return DoAngleStep(2, &servo2_target_angle);
}

/* Set servo position at specified speed */
int SetServo(int channel, int angle, int speed)
{
  assert_param(IS_VALID_SERVO(channel));
  assert_param(IS_VALID_ANGLE(angle));
  assert_param(IS_VALID_ANGLE_SPEED(speed));
  int step_ms = 1000 * ANGLE_STEP / speed;
  
  switch(channel)
  {
  case 1:
    servo1_target_angle = angle;
    AddScheduledTask(DoAngleStep1, step_ms);
    break;
  case 2:
    servo2_target_angle = angle;
    AddScheduledTask(DoAngleStep2, step_ms);
    break;
  }
  
  return GetServo(channel);
}

/* Get servo position */
int GetServo(int channel)
{ 
  int angle = -1;
  assert_param(IS_VALID_SERVO(channel));
  
  switch(channel)
  {
  case 1:
    angle = PWM_DUTY_TO_ANGLE(TIM2->CCR3);
    break;
  case 2:
    angle = PWM_DUTY_TO_ANGLE(TIM2->CCR4);
    break;
  }
  return angle;
}
